
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       arming_plane.c
  * @author     baiyang
  * @date       2023-7-7
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "arming_plane.h"

#include "fms.h"
#include "logger.h"
#include "define.h"
#include "quadplane.h"

#include <rtthread.h>

#include <notify/notify.h>
#include <logger/blog.h>
#include <logger/task_logger.h>

#include <ahrs/ahrs_view.h>
#include <gcs_mavlink/gcs.h>
#include <common/microbee.h>
#include <rc_channel/rc_channel.h>
#include <srv_channel/srv_channel.h>
#include <board_config/board_config.h>
/*-----------------------------------macro------------------------------------*/
// delay from arming to start of motor spoolup
#define AP_ARMING_DELAY_MS 2000
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool arming_plane_terrain_database_required();
static bool arming_plane_pre_arm_checks(bool display_failure);
static bool arming_plane_mandatory_checks(bool display_failure);
static bool arming_plane_ins_checks(bool display_failure);
static bool arming_plane_arm_checks(enum ArmingMethod method);
static bool arming_plane_arm(enum ArmingMethod method, const bool do_arming_checks);
static bool arming_plane_disarm(const enum ArmingMethod method, bool do_disarm_checks);
static bool arming_plane_mission_checks(bool report);
static bool rc_received_if_enabled_check(bool display_failure);
static void change_arm_state(void);
/*----------------------------------variable----------------------------------*/
static struct mb_arming_ops arming_ops = {.arm            = arming_plane_arm,
                                          .disarm         = arming_plane_disarm,
                                          .pre_arm_checks = arming_plane_pre_arm_checks,
                                          .arm_checks     = arming_plane_arm_checks,
                                          .barometer_checks = NULL,
                                          .ins_checks     = arming_plane_ins_checks,
                                          .gps_checks     = NULL,
                                          .board_voltage_checks = NULL,
                                          .rc_calibration_checks = NULL,
                                          .mission_checks = arming_plane_mission_checks,
                                          .terrain_database_required = arming_plane_terrain_database_required,
                                          .system_checks  = NULL,
                                          .proximity_checks = NULL,
                                          .mandatory_checks = arming_plane_mandatory_checks};

static struct mb_arming_plane arming_plane;
static struct mb_arming *arming = &arming_plane.parent;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mb_arming_update_soft_armed()
{
    bool _armed = mb_arming_is_armed();
#if HAL_QUADPLANE_ENABLED
    if (plane.quadplane.motor_test.running){
        _armed = true;
    }
#endif

    brd_set_soft_armed(_armed);

    // update delay_arming oneshot
    if (arming_plane.delay_arming &&
        (time_millis() - brd_get_last_armed_change() >= AP_ARMING_DELAY_MS)) {

        arming_plane.delay_arming = false;
    }
}

bool mb_arming_get_delay_arming()
{
    return arming_plane.delay_arming;
}

// expected to return true if the terrain database is required to have
// all data loaded
static bool arming_plane_terrain_database_required()
{
#if AP_TERRAIN_AVAILABLE
    if (fms.g.terrain_follow) {
        return true;
    }
#endif
    return mb_arming_TerrainDatabaseRequired();
}

/*
  additional arming checks for plane

 */
static bool arming_plane_pre_arm_checks(bool display_failure)
{
    if (arming->armed || arming->require == (int8_t)ARMING_REQUIRED_NO) {
        // if we are already armed or don't need any arming checks
        // then skip the checks
        return true;
    }
    //are arming checks disabled?
    if (arming->checks_to_perform == 0) {
        return mb_arming_mandatory_checks(display_failure);
    }

    // call parent class checks
    bool ret = mb_arming_PreArmChecks(display_failure);

    // Check airspeed sensor
    ret &= mb_arming_airspeed_checks(display_failure);

    if (fms.g.fs_timeout_long < fms.g.fs_timeout_short && fms.g.fs_action_short != FS_ACTION_SHORT_DISABLED) {
        mb_arming_check_failed(display_failure, "FS_LONG_TIMEOUT < FS_SHORT_TIMEOUT");
        ret = false;
    }

    if (fms.aparm.roll_limit_cd < 300) {
        mb_arming_check_failed(display_failure, "LIM_ROLL_CD too small (%u)", (unsigned)fms.aparm.roll_limit_cd);
        ret = false;
    }

    if (fms.aparm.pitch_limit_max_cd < 300) {
        mb_arming_check_failed(display_failure, "LIM_PITCH_MAX too small (%u)", (unsigned)fms.aparm.pitch_limit_max_cd);
        ret = false;
    }

    if (fms.aparm.pitch_limit_min_cd > -300) {
        mb_arming_check_failed(display_failure, "LIM_PITCH_MIN too large (%u)", (unsigned)fms.aparm.pitch_limit_min_cd);
        ret = false;
    }

    if (fms.aparm.airspeed_min < MIN_AIRSPEED_MIN) {
        mb_arming_check_failed(display_failure, "ARSPD_FBW_MIN too low (%i < %i)", fms.aparm.airspeed_min, MIN_AIRSPEED_MIN);
        ret = false;
    }

    if (rc_get_reverse(fms.channel_throttle) && 
        (enum ThrFailsafe)(fms.g.throttle_fs_enabled) != TFS_Disabled &&
        fms.g.throttle_fs_value < 
        rc_get_radio_max(fms.channel_throttle)) {
        mb_arming_check_failed(display_failure, "Invalid THR_FS_VALUE for rev throttle");
        ret = false;
    }

    ret &= rc_received_if_enabled_check(display_failure);

#if HAL_QUADPLANE_ENABLED
    ret &= quadplane_checks(display_failure);
#endif

    // check adsb avoidance failsafe
    if (fms.failsafe.adsb) {
        mb_arming_check_failed(display_failure, "ADSB threat detected");
        ret = false;
    }

    if (fms_flight_option_enabled(FO_CENTER_THROTTLE_TRIM)){
       int16_t trim = rc_get_radio_trim(fms.channel_throttle);
       if (trim < 1250 || trim > 1750) {
           mb_arming_check_failed(display_failure, "Throttle trim not near center stick(%u)",trim );
           ret = false;
       }
    }

    if (mission_get_in_landing_sequence_flag() &&
        !mission_starts_with_takeoff_cmd()) {
        mb_arming_check_failed(display_failure,"In landing sequence");
        ret = false;
    }

    char failure_msg[50] = {0};
    if (!mode_pre_arm_checks(fms.control_mode, ARRAY_SIZE(failure_msg), failure_msg)) {
        mb_arming_check_failed(display_failure, "%s %s", mode_name(fms.control_mode), failure_msg);
        return false;
    }

    return ret;
}

static bool arming_plane_mandatory_checks(bool display_failure)
{
    bool ret = true;

    ret &= rc_received_if_enabled_check(display_failure);

    // Call parent class checks
    ret &= mb_arming_MandatoryChecks(display_failure);

    return ret;
}

static bool arming_plane_ins_checks(bool display_failure)
{
    // call parent class checks
    if (!mb_arming_InsChecks(display_failure)) {
        return false;
    }

#if 0
    // additional plane specific checks
    if (mb_arming_check_enabled(ARMING_CHECK_INS)) {
        char failure_msg[50] = {0};
        if (!ahrs_pre_arm_check(true, failure_msg, sizeof(failure_msg))) {
            check_failed(ARMING_CHECK_INS, display_failure, "AHRS: %s", failure_msg);
            return false;
        }
    }
#endif

    return true;
}

static bool arming_plane_arm_checks(enum ArmingMethod method)
{
    if (method == ARMING_METHOD_RUDDER) {
        const enum RudderArming arming_rudder = mb_arming_get_rudder_arming_type();

        if (arming_rudder == ARMING_RUDDER_IS_DISABLED) {
            //parameter disallows rudder arming/disabling

            // if we emit a message here then someone doing surface
            // checks may be bothered by the message being emitted.
            // check_failed(true, "Rudder arming disabled");
            return false;
        }

        // if throttle is not down, then pilot cannot rudder arm/disarm
        if (!math_flt_zero(fms_get_throttle_input(false))){
            mb_arming_check_failed(true, "Non-zero throttle");
            return false;
        }
    }

    //are arming checks disabled?
    if (arming->checks_to_perform == 0) {
        return true;
    }

    // call parent class checks
    return mb_arming_ArmChecks(method);
}

static bool arming_plane_arm(enum ArmingMethod method, const bool do_arming_checks)
{
    if (!mb_armingArm(method, do_arming_checks)) {
        return false;
    }

    // let logger know that we're armed (it may open logs e.g.)
    if (blog_get_log_mode() == 0 || 
        blog_get_log_mode() == 1) {
        logger_start_blog(NULL);
    }

    if (fms_update_home()) {
        // after update_home the home position could still be
        // different from the current_loc if the EKF refused the
        // resetHeightDatum call. If we are updating home then we want
        // to force the home to be the current_loc so relative alt
        // takeoffs work correctly
        if (ahrs_set_home(fms.ahrs, &fms.current_loc)) {
            // update current_loc
            fms_update_current_loc();
        }
    }

    change_arm_state();

    fms.ahrs->arming_altitude_m = ahrs_get_position_z_up_cm(fms.ahrs) * 0.01f;

    //
    fms_actuator_armed_notify(true);

    // rising edge of delay_arming oneshot
    arming_plane.delay_arming = true;

    gcs_send_text(MAV_SEVERITY_INFO, "Throttle armed");

    mb_arming_log_write_arm(do_arming_checks, method);

    return true;
}

/*
  disarm motors
 */
static bool arming_plane_disarm(const enum ArmingMethod method, bool do_disarm_checks)
{
    if (do_disarm_checks &&
        (method == ARMING_METHOD_MAVLINK ||
         method == ARMING_METHOD_RUDDER)) {
        if (fms_is_flying()) {
            // don't allow mavlink or rudder disarm while flying
            return false;
        }
    }
    
    if (do_disarm_checks && method == ARMING_METHOD_RUDDER) {
        // option must be enabled:
        if (mb_arming_get_rudder_arming_type() != ARMING_RUDDER_ARMDISARM) {
            gcs_send_text(MAV_SEVERITY_INFO, "Rudder disarm: disabled");
            return false;
        }
    }

    if (!mb_armingDisarm(method, do_disarm_checks)) {
        return false;
    }
    if (fms.control_mode != (mode_base_t)&fms.mode_auto) {
        // reset the mission on disarm if we are not in auto
        mission_reset();
    }

    // suppress the throttle in auto-throttle modes
    fms.throttle_suppressed = mode_does_auto_throttle(fms.control_mode);

    // if no airmode switch assigned, ensure airmode is off:
#if HAL_QUADPLANE_ENABLED
    if ((plane.quadplane.air_mode == AirMode::ON) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr)) {
        plane.quadplane.air_mode = AirMode::OFF;
    }
#endif

    //only log if disarming was successful
    change_arm_state();

#if QAUTOTUNE_ENABLED
    //save qautotune gains if enabled and success
    if (plane.control_mode == &plane.mode_qautotune) {
        plane.quadplane.qautotune.save_tuning_gains();
    } else {
        plane.quadplane.qautotune.reset();
    }
#endif

    //
    fms_actuator_armed_notify(false);

    // re-initialize speed variable used in AUTO and GUIDED for
    // DO_CHANGE_SPEED commands
    fms.new_airspeed_cm = -1;
    
    gcs_send_text(MAV_SEVERITY_INFO, "Throttle disarmed");

    mb_arming_log_write_disarm(do_disarm_checks, method);

    if (blog_get_log_mode() == 0) {
        logger_stop_blog();
    }

    return true;
}

/*
  extra plane mission checks
 */
static bool arming_plane_mission_checks(bool report)
{
    // base checks
    bool ret = mb_arming_MissionChecks(report);
    if (mission_get_landing_sequence_start() > 0 && fms.g.rtl_autoland == RAL_RTL_DISABLE) {
        ret = false;
        mb_arming_check_failed2(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
    }
#if HAL_QUADPLANE_ENABLED
    if (plane.quadplane.available()) {
        const uint16_t num_commands = plane.mission.num_commands();
        AP_Mission::Mission_Command prev_cmd {};
        for (uint16_t i=1; i<num_commands; i++) {
            AP_Mission::Mission_Command cmd;
            if (!plane.mission.read_cmd_from_storage(i, cmd)) {
                break;
            }
            if (plane.is_land_command(cmd.id) &&
                prev_cmd.id == MAV_CMD_NAV_WAYPOINT) {
                const float dist = cmd.content.location.get_distance(prev_cmd.content.location);
                const float tecs_land_speed = plane.TECS_controller.get_land_airspeed();
                const float landing_speed = is_positive(tecs_land_speed)?tecs_land_speed:plane.aparm.airspeed_cruise_cm*0.01;
                const float min_dist = 0.75 * plane.quadplane.stopping_distance(sq(landing_speed));
                if (dist < min_dist) {
                    ret = false;
                    check_failed(ARMING_CHECK_MISSION, report, "VTOL land too short, min %.0fm", min_dist);
                }
            }
            prev_cmd = cmd;
        }
    }
#endif
    return ret;
}

// Checks rc has been received if it is configured to be used
static bool rc_received_if_enabled_check(bool display_failure)
{
    if (rcs_enabled_protocols() == 0) {
        // No protocols enabled, will never get RC, don't block arming
        return true;
    }

    // If RC failsafe is enabled we must receive RC before arming
    if (((enum ThrFailsafe)(fms.g.throttle_fs_enabled) == TFS_Enabled) && 
        !(rcs_has_had_rc_receiver() || rcs_has_had_rc_override())) {
        mb_arming_check_failed(display_failure, "Waiting for RC");
        return false;
    }

    return true;
}

/*
  update HAL soft arm state
*/
static void change_arm_state(void)
{
    mb_arming_update_soft_armed();
#if HAL_QUADPLANE_ENABLED
    plane.quadplane.set_armed(is_armed_and_safety_off());
#endif
}

// 
static int arming_plane_ctor(void)
{
    mb_arming_ctor((mb_arming_t)&arming_plane, &arming_ops);

    return 0;
}
MB_INIT_COMPONENT_EXPORT(arming_plane_ctor);
/*------------------------------------test------------------------------------*/


